function [dctr_mass_r, dctr_mass_xyz]=fsb_ctrm_3D_sandbox(idat,ref,disp2,sandbox)

% FSB : Calculate motion  with a Center of Mass Algorithm
%
% EXAMPLE:
% [dctr_mass_r, dctr_mass_xyz]=fsb_ctrm_3D_sandbox( idat,1,1,sandbox)
%
% INPUT:
% idat:   4D image data
% ref:      reference slice
% disp2:    should the trace be shown
% sandbox:  Experiment information struct
%
% OUTPUT;
% dctr_mass_r:      motion trace
% dctr_mass_xyz:    motion trace
%
% CALLED BY:
% FSB.m
%
% NOTES:
% Algorithm copyright George A Keliris 28-08-2006
% modified by Steffen Stoewer on 28-05-2008
%
% Copyright 2010 MPI for Biological Cybernetics
% License:GNU GPL, no express or implied warranties
% 
% $Revision 1.0
%
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

%~~~~~~~~~~~~~~~~~~~~~~
% Take voxel-mm transformation information from sandbox.hdr
%~~~~~~~~~~~~~~~~~~~~~~
hdr_x =sandbox.hdr.dime.pixdim(1,2);
hdr_y =sandbox.hdr.dime.pixdim(1,3);
hdr_z =sandbox.hdr.dime.pixdim(1,4);

h = waitbar(0,'Determining center of mass...');

%~~~~~~~~~~~~~~~~~~~~~~
% read in voxel size
%~~~~~~~~~~~~~~~~~~~~~~
mm=[hdr_x hdr_y hdr_z];

[xx yy zz tt]=size(idat);
[ix iy]=find(ones(xx,yy));
iz=repmat(1:zz,size(ix));
iz=iz(:);
ix=repmat(ix,zz,1);
iy=repmat(iy,zz,1);
linear_ind=find(ones(xx,yy,zz));

%~~~~~~~~~~~~~~~~~~~~~~
% convert coordinates to mm
%~~~~~~~~~~~~~~~~~~~~~~
ix=ix*mm(1);
iy=iy*mm(2);
iz=iz*mm(3);

for t=1:tt
    img=double(idat(:,:,:,t));
    w=img(linear_ind);
    waitbar(t/tt);
    ctr_mass_xyz(t,:)=sum([w.*ix w.*iy w.*iz]/sum(w));
end

dctr_mass_xyz(:,1)=ctr_mass_xyz(:,1)-ctr_mass_xyz(ref,1);
dctr_mass_xyz(:,2)=ctr_mass_xyz(:,2)-ctr_mass_xyz(ref,2);
dctr_mass_xyz(:,3)=ctr_mass_xyz(:,3)-ctr_mass_xyz(ref,3);
[th, phi,dctr_mass_r]=cart2sph(dctr_mass_xyz(:,1),dctr_mass_xyz(:,2),dctr_mass_xyz(:,3));

close(h);

if disp2 ==1;
    figure(500);
    
    plot(dctr_mass_r);
    xlabel('time -> [img]');
    ylabel('center-of-mass shift -> [mm]')
    figure(501);
    plot(dctr_mass_xyz);
    
end

end
